using BepuUtilities;
using BepuUtilities.Memory;
using BepuPhysics.Collidables;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using BepuPhysics.Trees;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.CollisionDetection
{
    public unsafe partial class BroadPhase : IDisposable
    {
        internal Buffer<CollidableReference> activeLeaves;
        internal Buffer<CollidableReference> staticLeaves;
        public BufferPool Pool { get; private set; }
        public Tree ActiveTree;
        public Tree StaticTree;
        Tree.RefitAndRefineMultithreadedContext activeRefineContext;
        // TODO：静电树不需要做几乎和活动树一样多的工作;这在未来将会改变。
        Tree.RefitAndRefineMultithreadedContext staticRefineContext;

        public BroadPhase(BufferPool pool, int initialActiveLeafCapacity = 4096, int initialStaticLeafCapacity = 8192)
        {
            Pool = pool;
            ActiveTree = new Tree(pool, initialActiveLeafCapacity);
            StaticTree = new Tree(pool, initialStaticLeafCapacity);
            pool.TakeAtLeast(initialActiveLeafCapacity, out activeLeaves);
            pool.TakeAtLeast(initialStaticLeafCapacity, out staticLeaves);

            activeRefineContext = new Tree.RefitAndRefineMultithreadedContext();
            staticRefineContext = new Tree.RefitAndRefineMultithreadedContext();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int Add(CollidableReference collidable, ref BoundingBox bounds, ref Tree tree, BufferPool pool, ref Buffer<CollidableReference> leaves)
        {
            var leafIndex = tree.Add(ref bounds, pool);
            if (leafIndex >= leaves.Length)
            {
                pool.ResizeToAtLeast(ref leaves, tree.LeafCount + 1, leaves.Length);
            }
            leaves[leafIndex] = collidable;
            return leafIndex;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool RemoveAt(int index, ref Tree tree, ref Buffer<CollidableReference> leaves, out CollidableReference movedLeaf)
        {
            Debug.Assert(index >= 0);
            var movedLeafIndex = tree.RemoveAt(index);
            if (movedLeafIndex >= 0)
            {
                movedLeaf = leaves[movedLeafIndex];
                leaves[index] = movedLeaf;
                return true;
            }
            movedLeaf = new CollidableReference();
            return false;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int AddActive(CollidableReference collidable, ref BoundingBox bounds)
        {
            return Add(collidable, ref bounds, ref ActiveTree, Pool, ref activeLeaves);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool RemoveActiveAt(int index, out CollidableReference movedLeaf)
        {
            return RemoveAt(index, ref ActiveTree, ref activeLeaves, out movedLeaf);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int AddStatic(CollidableReference collidable, ref BoundingBox bounds)
        {
            return Add(collidable, ref bounds, ref StaticTree, Pool, ref staticLeaves);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool RemoveStaticAt(int index, out CollidableReference movedLeaf)
        {
            return RemoveAt(index, ref StaticTree, ref staticLeaves, out movedLeaf);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetBoundsPointers(int broadPhaseIndex, ref Tree tree, out Vector3* minPointer, out Vector3* maxPointer)
        {
            var leaf = tree.Leaves[broadPhaseIndex];
            var nodeChild = (&tree.Nodes.Memory[leaf.NodeIndex].A) + leaf.ChildIndex;
            minPointer = &nodeChild->Min;
            maxPointer = &nodeChild->Max;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetActiveBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer)
        {
            GetBoundsPointers(index, ref ActiveTree, out minPointer, out maxPointer);
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetStaticBoundsPointers(int index, out Vector3* minPointer, out Vector3* maxPointer)
        {
            GetBoundsPointers(index, ref StaticTree, out minPointer, out maxPointer);
        }

        /// <summary>
        /// 将更新的边界应用于给定树中的给定叶索引,重新调整树以使其匹配。
        /// </summary>
        /// <param name="BroadPhaseIndex">要更新的树中叶的索引。</param>
        /// <param name="tree">包含要更新的叶的树。</param>
        /// <param name="min">叶的新最小界限。</param>
        /// <param name="max">叶的新最大界限。</param>
        public unsafe static void UpdateBounds(int broadPhaseIndex, ref Tree tree, in Vector3 min, in Vector3 max)
        {
            GetBoundsPointers(broadPhaseIndex, ref tree, out var minPointer, out var maxPointer);
            *minPointer = min;
            *maxPointer = max;
            tree.RefitForNodeBoundsChange(tree.Leaves[broadPhaseIndex].NodeIndex);
        }

        /// <summary>
        /// 将更新的边界应用于给定的活动叶索引,重新调整树以进行匹配。
        /// </summary>
        /// <param name="BroadPhaseIndex">要更新的叶的索引。</param>
        /// <param name="min">叶的新最小界限。</param>
        /// <param name="max">叶的新最大界限。</param>
        public void UpdateActiveBounds(int broadPhaseIndex, in Vector3 min, in Vector3 max)
        {
            UpdateBounds(broadPhaseIndex, ref ActiveTree, min, max);
        }
        /// <summary>
        /// 将更新的边界应用于给定的活动叶索引,重新调整树以进行匹配。
        /// </summary>
        /// <param name="BroadPhaseIndex">要更新的叶的索引。</param>
        /// <param name="min">叶的新最小界限。</param>
        /// <param name="max">叶的新最大界限。</param>
        public void UpdateStaticBounds(int broadPhaseIndex, in Vector3 min, in Vector3 max)
        {
            UpdateBounds(broadPhaseIndex, ref StaticTree, min, max);
        }

        int frameIndex;
        public void Update(IThreadDispatcher threadDispatcher = null)
        {
            if (frameIndex == int.MaxValue)
                frameIndex = 0;
            if (threadDispatcher != null)
            {
                activeRefineContext.RefitAndRefine(ref ActiveTree, Pool, threadDispatcher, frameIndex);
            }
            else
            {
                ActiveTree.RefitAndRefine(Pool, frameIndex);
            }

            // TODO：目前,非活动/静电树只是像另一个活动树一样进行更新。与理想情况相比,这是非常低效的-
            // 本质上,静电和非活动对象不会每帧都移动！
            // 这应该被专用的非活动/静电精细化方法所取代。它还应该与活动树并行运行,以提取更多的并行性;
            // 换句话说,从这两棵树生成工作,并将它们一起分派到所有树上。没有内部调度。
            if (threadDispatcher != null)
            {
                staticRefineContext.RefitAndRefine(ref StaticTree, Pool, threadDispatcher, frameIndex);
            }
            else
            {
                StaticTree.RefitAndRefine(Pool, frameIndex);
            }
            ++frameIndex;
        }

        /// <summary>
        /// 在不释放任何资源的情况下清除宽广阶段的结构。
        /// </summary>
        public void Clear()
        {
            ActiveTree.Clear();
            StaticTree.Clear();
        }

        void EnsureCapacity(ref Tree tree, ref Buffer<CollidableReference> leaves, int capacity)
        {
            if (tree.Leaves.Length < capacity)
                tree.Resize(Pool, capacity);
            if (leaves.Length < capacity)
                Pool.ResizeToAtLeast(ref leaves, capacity, tree.LeafCount);
        }
        void ResizeCapacity(ref Tree tree, ref Buffer<CollidableReference> leaves, int capacity)
        {
            capacity = Math.Max(capacity, tree.LeafCount);
            if (tree.Leaves.Length != BufferPool.GetCapacityForCount<Leaf>(capacity))
                tree.Resize(Pool, capacity);
            if (leaves.Length != BufferPool.GetCapacityForCount<CollidableReference>(capacity))
                Pool.ResizeToAtLeast(ref leaves, capacity, tree.LeafCount);
        }
        void Dispose(ref Tree tree, ref Buffer<CollidableReference> leaves)
        {
            Pool.Return(ref leaves);
            tree.Dispose(Pool);
        }
        /// <summary>
        /// 确保宽阔的相结构可以容纳至少给定数量的叶子。
        /// </summary>
        /// <param name="activeCapacity">要在活动树中为其分配空间的树叶数。</param>
        /// <param name="StaticCapacity">静电树中要分配空间的叶数。</param>
        public void EnsureCapacity(int activeCapacity, int staticCapacity)
        {
            EnsureCapacity(ref ActiveTree, ref activeLeaves, activeCapacity);
            EnsureCapacity(ref StaticTree, ref staticLeaves, staticCapacity);
        }

        /// <summary>
        /// 调整宽广相结构的大小以容纳给定数量的叶子。请注意,这是保守的;它永远不会遗弃任何现有的叶子。
        /// </summary>
        /// <param name="activeCapacity">要在活动树中为其分配空间的树叶数。</param>
        /// <param name="StaticCapacity">静电树中要分配空间的叶数。</param>
        public void Resize(int activeCapacity, int staticCapacity)
        {
            ResizeCapacity(ref ActiveTree, ref activeLeaves, activeCapacity);
            ResizeCapacity(ref StaticTree, ref staticLeaves, staticCapacity);
        }

        /// <summary>
        /// 释放宽阶段使用的内存。使得宽阔的阶段不可用。
        /// </summary>
        public void Dispose()
        {
            Dispose(ref ActiveTree, ref activeLeaves);
            Dispose(ref StaticTree, ref staticLeaves);
        }

    }
}
